#ifndef __PATH_SMOOTH_H__
#define __PATH_SMOOTH_H__

#include <iostream>
#include <geometry_msgs/Pose.h>
#include "path_generate/smoother/five_point_gradient.h"
using namespace std;

enum class SmoothMethod{
    FIVE_POINT_GRADIENT_DESCENT = 1,
};

class PathSmoother{
    public:
        PathSmoother(){};
        ~PathSmoother() = default;

        // 平滑路径
        void pathSmooth(SmoothMethod method, vector<geometry_msgs::Pose> poses_in, vector<geometry_msgs::Pose>& poses_out,
            std::function<bool(double x, double y)> f){
            switch (method){
                case SmoothMethod::FIVE_POINT_GRADIENT_DESCENT:
                    LOG(INFO) << "SmoothMethod: FIVE_POINT_GRADIENT_DESCENT";
                    five_point_gradient_descent_.pathSmooth(poses_in, poses_out, f);
                    break;
                default:
                    break;
            }

        };
    
    private:
        FivePointGradientDescent five_point_gradient_descent_;
};

#endif
